//k210部分
int color_x = 0;
int color_y = 0;
int color_w = 0;
int color_h = 0;
int search_for_color_num_red = 0;
int search_for_color_num_yellow = 0;

void k210_serial_init(){
  wk = malloc(sizeof(WonderK210));//分配内存
  //开启串口接收
  //这里不需要再调用初始化串口函数 Serial.begin(115200); 在 serial_begin() 函数里已经初始化了  
  wk->serial_begin();
  result = malloc(sizeof(Find_Box_Msg_st));//分配内存
}

//机械臂颜色分拣程序，当放置绿色物体时，机械臂抓取物体并放置到右侧，当放置蓝色物体时，机械臂抓取物体并放置到左侧.
void color_detection(){
  wk->update_data();//更新消息
  bool rt = wk->recive_box_msg(result , K210_COLOR_SORTING);
  if(rt){
    // Serial.print("color:"); Serial.println(result->msg);
    // Serial.print("x:"); Serial.print(result->x); Serial.print(" ");
    // Serial.print("y:"); Serial.print(result->y); Serial.print(" ");
    // Serial.print("w:"); Serial.print(result->w); Serial.print(" ");
    // Serial.print("h:"); Serial.print(result->h); Serial.print(" ");
    if(strcmp(result->msg, "Red") == 0) {
      Serial.print("I found red. ");
    }
    if(strcmp(result->msg, "Green") == 0){
      Serial.print("I found green. ");
      search_for_color_num_yellow = search_for_color_num_yellow + 1;
      if(search_for_color_num_yellow > 10){
        search_for_color_num_yellow = 0;
        delay(1000);
        put_right();
      }
    }
    if(strcmp(result->msg, "Blue") == 0){
      Serial.print("I found blue. ");
      search_for_color_num_red = search_for_color_num_red + 1;
      if(search_for_color_num_red>10){
        search_for_color_num_red = 0;
        delay(1000);
        put_left();
      }
    }
    if(strcmp(result->msg, "Yellow") == 0){
      Serial.print("I found yellow. ");
    }
    if(strcmp(result->msg, "Purple") == 0){
      Serial.print("I found purple. ");
    }
    color_x = result->x;
    color_y = result->y;
    color_w = result->w;
    color_h = result->h;
    Serial.print("(x,y,w,h)=");Serial.print("(");
    Serial.print(color_x);Serial.print(",");
    Serial.print(color_y);Serial.print(",");
    Serial.print(color_w);Serial.print(",");
    Serial.print(color_h);Serial.println(")");
    wk->clear_rec();
  }
  else{
    //Serial.println("Sorry, I not found color");
    delay(1);
  }
}


void k210_test(){
  wk->update_data();//更新消息
  bool rt = wk->recive_box_msg(result , K210_COLOR_SORTING);
  if(rt){
    // Serial.print("color:"); Serial.println(result->msg);
    // Serial.print("x:"); Serial.print(result->x); Serial.print(" ");
    // Serial.print("y:"); Serial.print(result->y); Serial.print(" ");
    // Serial.print("w:"); Serial.print(result->w); Serial.print(" ");
    // Serial.print("h:"); Serial.print(result->h); Serial.print(" ");
    if(strcmp(result->msg, "Red") == 0) {
      Serial.print("I found red. ");
    }
    if(strcmp(result->msg, "Green") == 0){
      Serial.print("I found green. ");
    }
    if(strcmp(result->msg, "Blue") == 0){
      Serial.print("I found blue. ");
    }
    if(strcmp(result->msg, "Yellow") == 0){
      Serial.print("I found yellow. ");
    }
    if(strcmp(result->msg, "Purple") == 0){
      Serial.print("I found purple. ");
    }
    color_x = result->x;
    color_y = result->y;
    color_w = result->w;
    color_h = result->h;
    Serial.print("(x,y,w,h)=");Serial.print("(");
    Serial.print(color_x);Serial.print(",");
    Serial.print(color_y);Serial.print(",");
    Serial.print(color_w);Serial.print(",");
    Serial.print(color_h);Serial.println(")");
    wk->clear_rec();
  }
  else{
    //Serial.println("Sorry, I not found color");
    delay(1);
  }
}